#ifndef POSTPROCESSOR_H_
#define POSTPROCESSOR_H_

#include <Eigen/Core>
#include <memory>
#include <mutex>
#include <vector>

#include "common/map_info.h"
#include "common/map_road_marking_defs.h"
#include "common/map_utility.h"
#include "common/utility.h"
#include "fit/dbscan.h"
#include "fit/rectangle_fitting.h"
#include "track/file_helper.h"
#include "track/matcher.h"
#include "track/object_track.h"

namespace autodrive {
namespace perception {

using ClusterResult = class Postprocessor {
 public:
  Postprocessor(/* args */);
  Postprocessor(Postprocessor const&) = delete;
  Postprocessor& operator=(Postprocessor const&) = delete;
  Postprocessor(Postprocessor&&) = delete;
  Postprocessor& operator=(Postprocessor&&) = delete;

  ~Postprocessor();

  /// @brief Init
  /// @param
  /// @return void
  void Init(const MapTrackParameter& settings,
            const ProcessCategory& cat) noexcept;

  /// @brief run processor
  /// @param
  /// @return void
  void Process(autodrive::perception::Map& map,
               autodrive::perception::MapElementSet<RoadMarkingResult>&
                   output_roadmarking,
               std::vector<Eigen::Vector2f>& ps, const Eigen::Affine3d& pos,
               const MapTrackParameter& settings) noexcept;

  /// @brief run processor
  /// @param
  /// @return void
  inline void SetSettings(const MapTrackParameter& para) noexcept {
    settings_ = para;
  };

 private:
  ///@brief mutex , multi thread safe
  std::mutex mutex_;
  ///@brief init flag
  bool is_init_;
  ///@brief tracker init flag
  bool track_initialization_flag_;
  ///@brief settings
  MapTrackParameter settings_;
  /// @brief fittings object
  std::unique_ptr<RectangleFitting> fitting_;
  /// @brief tracker Set
  ObjectTrackSet object_tracks_;
  /// @brief hungarian matcher
  std::unique_ptr<Mathcer> matcher_;
  /// @brief previous pose
  Eigen::Affine3d pre_pose_;
  /// @brief current pose
  Eigen::Affine3d cur_pose_;
  /// @brief hungarian matcher
  std::unique_ptr<DBScan<Eigen::Vector2f>> dbscan_;

  /// @brief construct track objects
  /// @param
  /// @return void
  void construct_tracked_objects(
      const vector<vector<Eigen::Vector2f>>& points,
      const vector<RotatedRect>& rects, const vector<cv::Mat>& mats,
      std::vector<TrackedObjectPtr>* tracked_objects) noexcept;

  /// @brief create trackerobject set
  /// @param
  /// @return void
  void create_new_tracks(
      std::vector<TrackedObjectPtr> const& new_objects,
      std::vector<std::int32_t> const& unassigned_objects_index,
      ObjectTrackSet& object_tracks_set);

  /// @brief create trackerobject set
  /// @param
  /// @return void
  void range(const std::vector<TrackedObjectPtr>& tracked_objects,
             std::vector<std::int32_t>& objects_index) noexcept;

  /// @brief update assigned tracks
  /// @params[IN] tracks_predict: predicted states of maintained tracks
  /// @params[IN] new_objects: recently detected objects
  /// @params[IN] assignments: assignment pair of <track, object>
  /// @params[IN] time_diff: time interval for updating
  /// @return nothing
  void update_assigned_tracks(std::vector<TrackedObjectPtr>* new_objects,
                              std::vector<TrackObjectPair> const& assignments,
                              double time_diff,
                              ObjectTrackSet& object_tracks_set);

  /// @brief update tracks without matched objects
  /// @param[IN] tracks_predict: predicted state of maintained tracks
  /// @param[IN] unassigned_tracks: index of unassigned tracks
  /// @param[IN]
  /// @param[IN]
  /// @param[IN] time_diff: time interval for updating
  /// @return nothing
  void update_unassigned_tracks(
      std::vector<Eigen::VectorXf> const& tracks_predict,
      std::vector<std::int32_t> const& unassigned_tracks, double time_diff,
      ObjectTrackSet& object_tracks_set);

  /// @brief delete lost tracks
  /// @return nothing
  void delete_lost_tracks(ObjectTrackSet& object_tracks_set);

  /// @brief compute cos theta from xy
  void set_zero_predict(std::vector<Eigen::VectorXf>& tracks_predict,
                        int track_num);

  /// @brief collect tracked results
  /// @param[OUT] lidar_objects: tracked objects with tracking information
  /// @return nothing
  void collect_tracked_results(
      autodrive::perception::Map& perceptionResult,
      autodrive::perception::MapElementSet<RoadMarkingResult>&
          output_roadmarking,
      ObjectTrackSet& object_tracks_set) noexcept;
};
}  // namespace perception
}  // namespace autodrive

#endif